<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.11"/>
<title>Cheetah Software: test_mini_cheetah_model.cpp Source File</title>
<link href="../../tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../jquery.js"></script>
<script type="text/javascript" src="../../dynsections.js"></script>
<link href="../../navtree.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../resize.js"></script>
<script type="text/javascript" src="../../navtreedata.js"></script>
<script type="text/javascript" src="../../navtree.js"></script>
<script type="text/javascript">
  $(document).ready(initResizable);
  $(window).load(resizeHeight);
</script>
<link href="../../search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../search/searchdata.js"></script>
<script type="text/javascript" src="../../search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { init_search(); });
</script>
<link href="../../doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">Cheetah Software
   &#160;<span id="projectnumber">1.0</span>
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.11 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "../../search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="../../index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="../../namespaces.html"><span>Namespaces</span></a></li>
      <li><a href="../../annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="../../files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="../../search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="../../search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
  <div id="navrow2" class="tabs2">
    <ul class="tablist">
      <li><a href="../../files.html"><span>File&#160;List</span></a></li>
      <li><a href="../../globals.html"><span>File&#160;Members</span></a></li>
    </ul>
  </div>
</div><!-- top -->
<div id="side-nav" class="ui-resizable side-nav-resizable">
  <div id="nav-tree">
    <div id="nav-tree-contents">
      <div id="nav-sync" class="sync"></div>
    </div>
  </div>
  <div id="splitbar" style="-moz-user-select:none;" 
       class="ui-resizable-handle">
  </div>
</div>
<script type="text/javascript">
$(document).ready(function(){initNavTree('d6/d6e/test__mini__cheetah__model_8cpp_source.html','../../');});
</script>
<div id="doc-content">
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div class="header">
  <div class="headertitle">
<div class="title">test_mini_cheetah_model.cpp</div>  </div>
</div><!--header-->
<div class="contents">
<a href="../../d6/d6e/test__mini__cheetah__model_8cpp.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;</div><div class="line"><a name="l00007"></a><span class="lineno">    7</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="../../d0/d52/_dynamics_simulator_8h.html">Dynamics/DynamicsSimulator.h</a>&quot;</span></div><div class="line"><a name="l00008"></a><span class="lineno">    8</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="../../d5/d9a/_floating_base_model_8h.html">Dynamics/FloatingBaseModel.h</a>&quot;</span></div><div class="line"><a name="l00009"></a><span class="lineno">    9</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="../../d8/db6/_mini_cheetah_8h.html">Dynamics/MiniCheetah.h</a>&quot;</span></div><div class="line"><a name="l00010"></a><span class="lineno">   10</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="../../d3/dc7/_quadruped_8h.html">Dynamics/Quadruped.h</a>&quot;</span></div><div class="line"><a name="l00011"></a><span class="lineno">   11</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="../../de/df0/utilities_8h.html">Utilities/utilities.h</a>&quot;</span></div><div class="line"><a name="l00012"></a><span class="lineno">   12</span>&#160;</div><div class="line"><a name="l00013"></a><span class="lineno">   13</span>&#160;<span class="preprocessor">#include &lt;iostream&gt;</span></div><div class="line"><a name="l00014"></a><span class="lineno">   14</span>&#160;</div><div class="line"><a name="l00015"></a><span class="lineno">   15</span>&#160;<span class="preprocessor">#include &quot;gmock/gmock.h&quot;</span></div><div class="line"><a name="l00016"></a><span class="lineno">   16</span>&#160;<span class="preprocessor">#include &quot;gtest/gtest.h&quot;</span></div><div class="line"><a name="l00017"></a><span class="lineno">   17</span>&#160;</div><div class="line"><a name="l00018"></a><span class="lineno">   18</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="../../d3/d68/_utilities__print_8h.html">Utilities/Utilities_print.h</a>&quot;</span></div><div class="line"><a name="l00019"></a><span class="lineno">   19</span>&#160;</div><div class="line"><a name="l00020"></a><span class="lineno">   20</span>&#160;<span class="keyword">using namespace </span><a class="code" href="../../de/d1a/namespacespatial.html">spatial</a>;</div><div class="line"><a name="l00021"></a><span class="lineno">   21</span>&#160;</div><div class="line"><a name="l00025"></a><span class="lineno"><a class="line" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#aed75690b2aa3bde430ac05521cecb8d1">   25</a></span>&#160;<a class="code" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#aed75690b2aa3bde430ac05521cecb8d1">TEST</a>(MiniCheetah, miniCheetahModel1) {</div><div class="line"><a name="l00026"></a><span class="lineno">   26</span>&#160;  <a class="code" href="../../d6/db1/class_floating_base_model.html">FloatingBaseModel&lt;double&gt;</a> <a class="code" href="../../d7/dfa/namespacecheetah.html">cheetah</a> = buildMiniCheetah&lt;double&gt;().buildModel();</div><div class="line"><a name="l00027"></a><span class="lineno">   27</span>&#160;</div><div class="line"><a name="l00028"></a><span class="lineno">   28</span>&#160;  <span class="comment">// masses</span></div><div class="line"><a name="l00029"></a><span class="lineno">   29</span>&#160;  EXPECT_TRUE(<a class="code" href="../../de/df0/utilities_8h.html#a16ea9aab4737c2622bc370d6a8613d63">fpEqual</a>(8.2520, cheetah.<a class="code" href="../../d6/db1/class_floating_base_model.html#ac3e58feb2dceedb9739bc76bcaccbc98">totalNonRotorMass</a>(), .0001));</div><div class="line"><a name="l00030"></a><span class="lineno">   30</span>&#160;  EXPECT_TRUE(<a class="code" href="../../de/df0/utilities_8h.html#a16ea9aab4737c2622bc370d6a8613d63">fpEqual</a>(0.66, cheetah.<a class="code" href="../../d6/db1/class_floating_base_model.html#a76cd0fcd431c77e002d8e111b2880cbe">totalRotorMass</a>(), .0001));</div><div class="line"><a name="l00031"></a><span class="lineno">   31</span>&#160;</div><div class="line"><a name="l00032"></a><span class="lineno">   32</span>&#160;  <span class="comment">// check tree structure</span></div><div class="line"><a name="l00033"></a><span class="lineno">   33</span>&#160;  std::vector&lt;int&gt; parentRef{0, 0, 0,  0, 0,  0,  5, 6,  7,</div><div class="line"><a name="l00034"></a><span class="lineno">   34</span>&#160;                             5, 9, 10, 5, 12, 13, 5, 15, 16};</div><div class="line"><a name="l00035"></a><span class="lineno">   35</span>&#160;  EXPECT_TRUE(<a class="code" href="../../de/df0/utilities_8h.html#a42a85233a23e8e7af211cb76f5fce9ee">vectorEqual</a>(parentRef, cheetah.<a class="code" href="../../d6/db1/class_floating_base_model.html#a477c5193fa0c0f6e36314eca214a7248">getParentVector</a>()));</div><div class="line"><a name="l00036"></a><span class="lineno">   36</span>&#160;</div><div class="line"><a name="l00037"></a><span class="lineno">   37</span>&#160;  <span class="comment">// this is kind of stupid, but a reasonable sanity check for inertias</span></div><div class="line"><a name="l00038"></a><span class="lineno">   38</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;double&gt;</a> inertiaSumRef;</div><div class="line"><a name="l00039"></a><span class="lineno">   39</span>&#160;  inertiaSumRef &lt;&lt;</div><div class="line"><a name="l00040"></a><span class="lineno">   40</span>&#160;      <span class="comment">// 0.0272,      0, 0.0001,       0, 0.0663, 0,</span></div><div class="line"><a name="l00041"></a><span class="lineno">   41</span>&#160;      <span class="comment">// 0, 0.3758,    0.0, -0.0663,      0, 0,</span></div><div class="line"><a name="l00042"></a><span class="lineno">   42</span>&#160;      <span class="comment">// 0.0001, 0.0000, 0.0497, 0, 0, 0,</span></div><div class="line"><a name="l00043"></a><span class="lineno">   43</span>&#160;      <span class="comment">// 0, -0.0663, 0, 8.4170, 0, 0,</span></div><div class="line"><a name="l00044"></a><span class="lineno">   44</span>&#160;      <span class="comment">// 0.0663, 0, 0, 0, 8.4170, 0,</span></div><div class="line"><a name="l00045"></a><span class="lineno">   45</span>&#160;      <span class="comment">// 0, 0, 0, 0, 0, 8.4170;</span></div><div class="line"><a name="l00046"></a><span class="lineno">   46</span>&#160;</div><div class="line"><a name="l00047"></a><span class="lineno">   47</span>&#160;      0.0272,</div><div class="line"><a name="l00048"></a><span class="lineno">   48</span>&#160;      0, 0, 0, 0.0663, 0, 0, 0.05, 0, -0.066336, 0, 0, 0, 0, 0.0497, 0, 0, 0, 0,</div><div class="line"><a name="l00049"></a><span class="lineno">   49</span>&#160;      -0.066336, 0, 8.417, 0, 0, 0.066336, 0, 0, 0, 8.417, 0, 0, 0, 0, 0, 0,</div><div class="line"><a name="l00050"></a><span class="lineno">   50</span>&#160;      8.417;</div><div class="line"><a name="l00051"></a><span class="lineno">   51</span>&#160;</div><div class="line"><a name="l00052"></a><span class="lineno">   52</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;double&gt;</a> inertiaSum = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;double&gt;::Zero</a>();</div><div class="line"><a name="l00053"></a><span class="lineno">   53</span>&#160;</div><div class="line"><a name="l00054"></a><span class="lineno">   54</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 18; i++) {</div><div class="line"><a name="l00055"></a><span class="lineno">   55</span>&#160;    inertiaSum += cheetah.<a class="code" href="../../d6/db1/class_floating_base_model.html#a445bbce9a3f0a56164b06d39760d2383">getBodyInertiaVector</a>()[i].getMatrix() +</div><div class="line"><a name="l00056"></a><span class="lineno">   56</span>&#160;                  cheetah.<a class="code" href="../../d6/db1/class_floating_base_model.html#a4c77595286da687122899d5e61139305">getRotorInertiaVector</a>()[i].getMatrix() * 0.25;</div><div class="line"><a name="l00057"></a><span class="lineno">   57</span>&#160;  }</div><div class="line"><a name="l00058"></a><span class="lineno">   58</span>&#160;</div><div class="line"><a name="l00059"></a><span class="lineno">   59</span>&#160;  EXPECT_TRUE(<a class="code" href="../../de/dda/_math_utilities_8h.html#aa06ec79f1e3170ea41b58ffbf35b9fb6">almostEqual</a>(inertiaSum, inertiaSumRef, .0003));</div><div class="line"><a name="l00060"></a><span class="lineno">   60</span>&#160;}</div><div class="line"><a name="l00061"></a><span class="lineno">   61</span>&#160;</div><div class="line"><a name="l00065"></a><span class="lineno"><a class="line" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#abc1becd574334fc98167b33d0d72f6dc">   65</a></span>&#160;<a class="code" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#aed75690b2aa3bde430ac05521cecb8d1">TEST</a>(MiniCheetah, miniCheetahModel2) {</div><div class="line"><a name="l00066"></a><span class="lineno">   66</span>&#160;  <a class="code" href="../../d6/db1/class_floating_base_model.html">FloatingBaseModel&lt;double&gt;</a> <a class="code" href="../../d7/dfa/namespacecheetah.html">cheetah</a> = buildMiniCheetah&lt;double&gt;().buildModel();</div><div class="line"><a name="l00067"></a><span class="lineno">   67</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;double&gt;</a> XTotal = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;double&gt;::Identity</a>();</div><div class="line"><a name="l00068"></a><span class="lineno">   68</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;double&gt;</a> XRotTotal = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;double&gt;::Identity</a>();</div><div class="line"><a name="l00069"></a><span class="lineno">   69</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 18; i++) {</div><div class="line"><a name="l00070"></a><span class="lineno">   70</span>&#160;    XTotal = XTotal + cheetah.<a class="code" href="../../d6/db1/class_floating_base_model.html#afb96756832c10edf60bcb16a0a1589bd">_Xtree</a>[i];</div><div class="line"><a name="l00071"></a><span class="lineno">   71</span>&#160;    XRotTotal = XRotTotal + cheetah.<a class="code" href="../../d6/db1/class_floating_base_model.html#a647e76b4459b43335cf528795bab72d5">_Xrot</a>[i];</div><div class="line"><a name="l00072"></a><span class="lineno">   72</span>&#160;  }</div><div class="line"><a name="l00073"></a><span class="lineno">   73</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;double&gt;</a> Xtr, Xrtr;</div><div class="line"><a name="l00074"></a><span class="lineno">   74</span>&#160;  Xtr &lt;&lt; 11.0000, 0.0000, 0, 0, 0, 0, -0.0000, 11.0000, 0, 0, 0, 0, 0, 0,</div><div class="line"><a name="l00075"></a><span class="lineno">   75</span>&#160;      19.0000, 0, 0, 0, 0, -0.8360, 0, 11.0000, 0.0000, 0, 0.8360, 0, 0.0000,</div><div class="line"><a name="l00076"></a><span class="lineno">   76</span>&#160;      -0.0000, 11.0000, 0, 0, 0, 0, 0, 0, 19.0000;</div><div class="line"><a name="l00077"></a><span class="lineno">   77</span>&#160;</div><div class="line"><a name="l00078"></a><span class="lineno">   78</span>&#160;  Xrtr &lt;&lt; 11.0000, 0.0000, 0, 0, 0, 0, -0.0000, 11.0000, 0, 0, 0, 0, 0, 0,</div><div class="line"><a name="l00079"></a><span class="lineno">   79</span>&#160;      19.0000, 0, 0, 0, 0, 0, 0, 11.0000, 0.0000, 0, 0, 0, 0.0000, -0.0000,</div><div class="line"><a name="l00080"></a><span class="lineno">   80</span>&#160;      11.0000, 0, 0.0000, 0, 0, 0, 0, 19.0000;</div><div class="line"><a name="l00081"></a><span class="lineno">   81</span>&#160;</div><div class="line"><a name="l00082"></a><span class="lineno">   82</span>&#160;  EXPECT_TRUE(<a class="code" href="../../de/dda/_math_utilities_8h.html#aa06ec79f1e3170ea41b58ffbf35b9fb6">almostEqual</a>(Xtr, XTotal, .0005));</div><div class="line"><a name="l00083"></a><span class="lineno">   83</span>&#160;  EXPECT_TRUE(<a class="code" href="../../de/dda/_math_utilities_8h.html#aa06ec79f1e3170ea41b58ffbf35b9fb6">almostEqual</a>(Xrtr, XRotTotal, .0005));</div><div class="line"><a name="l00084"></a><span class="lineno">   84</span>&#160;}</div><div class="line"><a name="l00085"></a><span class="lineno">   85</span>&#160;</div><div class="line"><a name="l00090"></a><span class="lineno"><a class="line" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#a12907b098ba14de7388658b36621fbc5">   90</a></span>&#160;<a class="code" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#aed75690b2aa3bde430ac05521cecb8d1">TEST</a>(MiniCheetah, simulatorDynamicsDoesntCrashMiniCheetah) {</div><div class="line"><a name="l00091"></a><span class="lineno">   91</span>&#160;  <a class="code" href="../../d6/db1/class_floating_base_model.html">FloatingBaseModel&lt;double&gt;</a> <a class="code" href="../../d7/dfa/namespacecheetah.html">cheetah</a> = buildMiniCheetah&lt;double&gt;().buildModel();</div><div class="line"><a name="l00092"></a><span class="lineno">   92</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html">DynamicsSimulator&lt;double&gt;</a> sim(cheetah);</div><div class="line"><a name="l00093"></a><span class="lineno">   93</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> tau(12);</div><div class="line"><a name="l00094"></a><span class="lineno">   94</span>&#160;  sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a3c057386e8d437e8c5dbcae02acf935c">forwardKinematics</a>();</div><div class="line"><a name="l00095"></a><span class="lineno">   95</span>&#160;  sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#ab7c1b6d1eb8d4893458b6910b1f9d331">runABA</a>(tau);</div><div class="line"><a name="l00096"></a><span class="lineno">   96</span>&#160;}</div><div class="line"><a name="l00097"></a><span class="lineno">   97</span>&#160;</div><div class="line"><a name="l00103"></a><span class="lineno"><a class="line" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#aba5c397613e2936f6b222fb6cad75d9a">  103</a></span>&#160;<a class="code" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#aed75690b2aa3bde430ac05521cecb8d1">TEST</a>(MiniCheetah, simulatorDynamicsABANoExternalForceMiniCheetah) {</div><div class="line"><a name="l00104"></a><span class="lineno">  104</span>&#160;  <a class="code" href="../../d6/db1/class_floating_base_model.html">FloatingBaseModel&lt;double&gt;</a> cheetahModel =</div><div class="line"><a name="l00105"></a><span class="lineno">  105</span>&#160;      buildMiniCheetah&lt;double&gt;().buildModel();</div><div class="line"><a name="l00106"></a><span class="lineno">  106</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html">DynamicsSimulator&lt;double&gt;</a> sim(cheetahModel);</div><div class="line"><a name="l00107"></a><span class="lineno">  107</span>&#160;</div><div class="line"><a name="l00108"></a><span class="lineno">  108</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat&lt;double&gt;</a> rBody = <a class="code" href="../../da/d9b/namespaceori.html#a537b8d0769c03e4cd8ef2b9a9dc8a760">coordinateRotation</a>(CoordinateAxis::X, .123) *</div><div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;                         <a class="code" href="../../da/d9b/namespaceori.html#a537b8d0769c03e4cd8ef2b9a9dc8a760">coordinateRotation</a>(CoordinateAxis::Z, .232) *</div><div class="line"><a name="l00110"></a><span class="lineno">  110</span>&#160;                         <a class="code" href="../../da/d9b/namespaceori.html#a537b8d0769c03e4cd8ef2b9a9dc8a760">coordinateRotation</a>(CoordinateAxis::Y, .111);</div><div class="line"><a name="l00111"></a><span class="lineno">  111</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;double&gt;</a> bodyVel;</div><div class="line"><a name="l00112"></a><span class="lineno">  112</span>&#160;  bodyVel &lt;&lt; 1, 2, 3, 4, 5, 6;</div><div class="line"><a name="l00113"></a><span class="lineno">  113</span>&#160;  <a class="code" href="../../de/d75/struct_f_b_model_state.html">FBModelState&lt;double&gt;</a> x;</div><div class="line"><a name="l00114"></a><span class="lineno">  114</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> q(12);</div><div class="line"><a name="l00115"></a><span class="lineno">  115</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> dq(12);</div><div class="line"><a name="l00116"></a><span class="lineno">  116</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> tau(12);</div><div class="line"><a name="l00117"></a><span class="lineno">  117</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 12; i++) {</div><div class="line"><a name="l00118"></a><span class="lineno">  118</span>&#160;    q[i] = i + 1;</div><div class="line"><a name="l00119"></a><span class="lineno">  119</span>&#160;    dq[i] = (i + 1) * 2;</div><div class="line"><a name="l00120"></a><span class="lineno">  120</span>&#160;    tau[i] = (i + 1) * -3.;</div><div class="line"><a name="l00121"></a><span class="lineno">  121</span>&#160;  }</div><div class="line"><a name="l00122"></a><span class="lineno">  122</span>&#160;</div><div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;  <span class="comment">// set state</span></div><div class="line"><a name="l00124"></a><span class="lineno">  124</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#ae42e1f0d68b89af761bcaf273b97bca7">bodyOrientation</a> = <a class="code" href="../../da/d9b/namespaceori.html#a73c3872af6870d856cedd662856fcaea">rotationMatrixToQuaternion</a>(rBody.transpose());</div><div class="line"><a name="l00125"></a><span class="lineno">  125</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abf1fc0221d6b92d103193524a2046ffc">bodyVelocity</a> = bodyVel;</div><div class="line"><a name="l00126"></a><span class="lineno">  126</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a> = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a>(6, 7, 8);</div><div class="line"><a name="l00127"></a><span class="lineno">  127</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#ae5a66b0d3fecb2cdd9ee3c68f72e9182">q</a> = q;</div><div class="line"><a name="l00128"></a><span class="lineno">  128</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#a97f42a6014fc5d9fd3dc22f7158099f4">qd</a> = dq;</div><div class="line"><a name="l00129"></a><span class="lineno">  129</span>&#160;</div><div class="line"><a name="l00130"></a><span class="lineno">  130</span>&#160;  <span class="comment">// do aba</span></div><div class="line"><a name="l00131"></a><span class="lineno">  131</span>&#160;  sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a1b185d90d2b15840ca57f8e2adcf84fb">setState</a>(x);</div><div class="line"><a name="l00132"></a><span class="lineno">  132</span>&#160;  sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a3c057386e8d437e8c5dbcae02acf935c">forwardKinematics</a>();</div><div class="line"><a name="l00133"></a><span class="lineno">  133</span>&#160;  sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#ab7c1b6d1eb8d4893458b6910b1f9d331">runABA</a>(tau);</div><div class="line"><a name="l00134"></a><span class="lineno">  134</span>&#160;</div><div class="line"><a name="l00135"></a><span class="lineno">  135</span>&#160;  <span class="comment">// check:</span></div><div class="line"><a name="l00136"></a><span class="lineno">  136</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a> pdRef(4.3717, 4.8598, 5.8541);</div><div class="line"><a name="l00137"></a><span class="lineno">  137</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;double&gt;</a> vbdRef;</div><div class="line"><a name="l00138"></a><span class="lineno">  138</span>&#160;  vbdRef &lt;&lt; 1217.31, -182.793, -171.524, -4.09393, 33.6798, -59.975;</div><div class="line"><a name="l00139"></a><span class="lineno">  139</span>&#160;</div><div class="line"><a name="l00140"></a><span class="lineno">  140</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> qddRef(12);</div><div class="line"><a name="l00141"></a><span class="lineno">  141</span>&#160;  qddRef &lt;&lt; -1946.36, -1056.62, -1470.57, -3133.03, -1263.89, -2626.9, -6637.38,</div><div class="line"><a name="l00142"></a><span class="lineno">  142</span>&#160;      -4071.88, -4219.14, -7554.08, -3392.44, -5112.85;</div><div class="line"><a name="l00143"></a><span class="lineno">  143</span>&#160;</div><div class="line"><a name="l00144"></a><span class="lineno">  144</span>&#160;  EXPECT_TRUE(<a class="code" href="../../de/dda/_math_utilities_8h.html#aa06ec79f1e3170ea41b58ffbf35b9fb6">almostEqual</a>(pdRef, sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a1dc9f92d57994e053f91b9cb619adde4">getDState</a>().<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#a825cfc02adde2c48b0d508a3bf77e896">dBodyPosition</a>, .001));</div><div class="line"><a name="l00145"></a><span class="lineno">  145</span>&#160;  EXPECT_TRUE(<a class="code" href="../../de/dda/_math_utilities_8h.html#aa06ec79f1e3170ea41b58ffbf35b9fb6">almostEqual</a>(vbdRef, sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a1dc9f92d57994e053f91b9cb619adde4">getDState</a>().<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#a8b7b6d70f1b1268f02e1b291dad26c5d">dBodyVelocity</a>, 1));</div><div class="line"><a name="l00146"></a><span class="lineno">  146</span>&#160;</div><div class="line"><a name="l00147"></a><span class="lineno">  147</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 12; i++) {</div><div class="line"><a name="l00148"></a><span class="lineno">  148</span>&#160;    <span class="comment">// the qdd&#39;s are large - see qddRef, so we&#39;re only accurate to within ~1.</span></div><div class="line"><a name="l00149"></a><span class="lineno">  149</span>&#160;    EXPECT_TRUE(<a class="code" href="../../de/df0/utilities_8h.html#a16ea9aab4737c2622bc370d6a8613d63">fpEqual</a>(sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a1dc9f92d57994e053f91b9cb619adde4">getDState</a>().<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#af32a2041b72e6b30ed1c8a027d340fd2">qdd</a>[i], qddRef[i], 3.));</div><div class="line"><a name="l00150"></a><span class="lineno">  150</span>&#160;  }</div><div class="line"><a name="l00151"></a><span class="lineno">  151</span>&#160;}</div><div class="line"><a name="l00152"></a><span class="lineno">  152</span>&#160;</div><div class="line"><a name="l00159"></a><span class="lineno"><a class="line" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#ad731b2a7728aaddd413311a8ee520d3d">  159</a></span>&#160;<a class="code" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#aed75690b2aa3bde430ac05521cecb8d1">TEST</a>(MiniCheetah, simulatorDynamicsWithExternalForceMiniCheetah) {</div><div class="line"><a name="l00160"></a><span class="lineno">  160</span>&#160;  <a class="code" href="../../d6/db1/class_floating_base_model.html">FloatingBaseModel&lt;double&gt;</a> cheetahModel =</div><div class="line"><a name="l00161"></a><span class="lineno">  161</span>&#160;      buildMiniCheetah&lt;double&gt;().buildModel();</div><div class="line"><a name="l00162"></a><span class="lineno">  162</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html">DynamicsSimulator&lt;double&gt;</a> sim(cheetahModel, <span class="keyword">true</span>);</div><div class="line"><a name="l00163"></a><span class="lineno">  163</span>&#160;</div><div class="line"><a name="l00164"></a><span class="lineno">  164</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat&lt;double&gt;</a> rBody = <a class="code" href="../../da/d9b/namespaceori.html#a537b8d0769c03e4cd8ef2b9a9dc8a760">coordinateRotation</a>(CoordinateAxis::X, .123) *</div><div class="line"><a name="l00165"></a><span class="lineno">  165</span>&#160;                         <a class="code" href="../../da/d9b/namespaceori.html#a537b8d0769c03e4cd8ef2b9a9dc8a760">coordinateRotation</a>(CoordinateAxis::Z, .232) *</div><div class="line"><a name="l00166"></a><span class="lineno">  166</span>&#160;                         <a class="code" href="../../da/d9b/namespaceori.html#a537b8d0769c03e4cd8ef2b9a9dc8a760">coordinateRotation</a>(CoordinateAxis::Y, .111);</div><div class="line"><a name="l00167"></a><span class="lineno">  167</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;double&gt;</a> bodyVel;</div><div class="line"><a name="l00168"></a><span class="lineno">  168</span>&#160;  bodyVel &lt;&lt; 1, 2, 3, 4, 5, 6;</div><div class="line"><a name="l00169"></a><span class="lineno">  169</span>&#160;  <a class="code" href="../../de/d75/struct_f_b_model_state.html">FBModelState&lt;double&gt;</a> x;</div><div class="line"><a name="l00170"></a><span class="lineno">  170</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> q(12);</div><div class="line"><a name="l00171"></a><span class="lineno">  171</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> dq(12);</div><div class="line"><a name="l00172"></a><span class="lineno">  172</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> tau(12);</div><div class="line"><a name="l00173"></a><span class="lineno">  173</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 12; i++) {</div><div class="line"><a name="l00174"></a><span class="lineno">  174</span>&#160;    q[i] = i + 1;</div><div class="line"><a name="l00175"></a><span class="lineno">  175</span>&#160;    dq[i] = (i + 1) * 2;</div><div class="line"><a name="l00176"></a><span class="lineno">  176</span>&#160;    tau[i] = (i + 1) * -3.;</div><div class="line"><a name="l00177"></a><span class="lineno">  177</span>&#160;  }</div><div class="line"><a name="l00178"></a><span class="lineno">  178</span>&#160;</div><div class="line"><a name="l00179"></a><span class="lineno">  179</span>&#160;  <span class="comment">// set state</span></div><div class="line"><a name="l00180"></a><span class="lineno">  180</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#ae42e1f0d68b89af761bcaf273b97bca7">bodyOrientation</a> = <a class="code" href="../../da/d9b/namespaceori.html#a73c3872af6870d856cedd662856fcaea">rotationMatrixToQuaternion</a>(rBody.transpose());</div><div class="line"><a name="l00181"></a><span class="lineno">  181</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abf1fc0221d6b92d103193524a2046ffc">bodyVelocity</a> = bodyVel;</div><div class="line"><a name="l00182"></a><span class="lineno">  182</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a> = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a>(6, 7, 8);</div><div class="line"><a name="l00183"></a><span class="lineno">  183</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#ae5a66b0d3fecb2cdd9ee3c68f72e9182">q</a> = q;</div><div class="line"><a name="l00184"></a><span class="lineno">  184</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#a97f42a6014fc5d9fd3dc22f7158099f4">qd</a> = dq;</div><div class="line"><a name="l00185"></a><span class="lineno">  185</span>&#160;</div><div class="line"><a name="l00186"></a><span class="lineno">  186</span>&#160;  <span class="comment">// generate external forces</span></div><div class="line"><a name="l00187"></a><span class="lineno">  187</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a078e1eaa8a3af13cef27a0a8d0c1302e">vectorAligned&lt;SVec&lt;double&gt;</a>&gt; forces(18);</div><div class="line"><a name="l00188"></a><span class="lineno">  188</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 18; i++) {</div><div class="line"><a name="l00189"></a><span class="lineno">  189</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j = 0; j &lt; 6; j++) {</div><div class="line"><a name="l00190"></a><span class="lineno">  190</span>&#160;      forces[i][j] = .3 * (i + j + 1);</div><div class="line"><a name="l00191"></a><span class="lineno">  191</span>&#160;    }</div><div class="line"><a name="l00192"></a><span class="lineno">  192</span>&#160;  }</div><div class="line"><a name="l00193"></a><span class="lineno">  193</span>&#160;</div><div class="line"><a name="l00194"></a><span class="lineno">  194</span>&#160;  <span class="comment">// do aba</span></div><div class="line"><a name="l00195"></a><span class="lineno">  195</span>&#160;  sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a1b185d90d2b15840ca57f8e2adcf84fb">setState</a>(x);</div><div class="line"><a name="l00196"></a><span class="lineno">  196</span>&#160;  sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a2310bcadca5b6ed12ec9cee898335625">setAllExternalForces</a>(forces);</div><div class="line"><a name="l00197"></a><span class="lineno">  197</span>&#160;  sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a84b25acaf7edf23f4f437a96a0d8ba05">step</a>(0.0, tau, 5e5, 5e3);</div><div class="line"><a name="l00198"></a><span class="lineno">  198</span>&#160;</div><div class="line"><a name="l00199"></a><span class="lineno">  199</span>&#160;  <span class="comment">// check:</span></div><div class="line"><a name="l00200"></a><span class="lineno">  200</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a> pdRef(4.3717, 4.8598, 5.8541);</div><div class="line"><a name="l00201"></a><span class="lineno">  201</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;double&gt;</a> vbdRef;</div><div class="line"><a name="l00202"></a><span class="lineno">  202</span>&#160;  vbdRef &lt;&lt; 3153.46, 42.6931, 264.584, -3.80573, -53.3519, 68.5713;</div><div class="line"><a name="l00203"></a><span class="lineno">  203</span>&#160;</div><div class="line"><a name="l00204"></a><span class="lineno">  204</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> qddRef(12);</div><div class="line"><a name="l00205"></a><span class="lineno">  205</span>&#160;  qddRef &lt;&lt; -1211.85, -1167.39, -2024.94, 394.414, -297.854, -2292.29, -1765.92,</div><div class="line"><a name="l00206"></a><span class="lineno">  206</span>&#160;      -4040.78, -4917.41, 149.368, -2508.2, -4969.02;</div><div class="line"><a name="l00207"></a><span class="lineno">  207</span>&#160;</div><div class="line"><a name="l00208"></a><span class="lineno">  208</span>&#160;  EXPECT_TRUE(<a class="code" href="../../de/dda/_math_utilities_8h.html#aa06ec79f1e3170ea41b58ffbf35b9fb6">almostEqual</a>(pdRef, sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a1dc9f92d57994e053f91b9cb619adde4">getDState</a>().<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#a825cfc02adde2c48b0d508a3bf77e896">dBodyPosition</a>, .001));</div><div class="line"><a name="l00209"></a><span class="lineno">  209</span>&#160;  EXPECT_TRUE(<a class="code" href="../../de/dda/_math_utilities_8h.html#aa06ec79f1e3170ea41b58ffbf35b9fb6">almostEqual</a>(vbdRef, sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a1dc9f92d57994e053f91b9cb619adde4">getDState</a>().<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#a8b7b6d70f1b1268f02e1b291dad26c5d">dBodyVelocity</a>, .01));</div><div class="line"><a name="l00210"></a><span class="lineno">  210</span>&#160;</div><div class="line"><a name="l00211"></a><span class="lineno">  211</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 12; i++) {</div><div class="line"><a name="l00212"></a><span class="lineno">  212</span>&#160;    <span class="comment">// the qdd&#39;s are large - see qddRef, so we&#39;re only accurate to within ~1.</span></div><div class="line"><a name="l00213"></a><span class="lineno">  213</span>&#160;    EXPECT_TRUE(<a class="code" href="../../de/df0/utilities_8h.html#a16ea9aab4737c2622bc370d6a8613d63">fpEqual</a>(sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a1dc9f92d57994e053f91b9cb619adde4">getDState</a>().<a class="code" href="../../de/dce/struct_f_b_model_state_derivative.html#af32a2041b72e6b30ed1c8a027d340fd2">qdd</a>[i], qddRef[i], 3.));</div><div class="line"><a name="l00214"></a><span class="lineno">  214</span>&#160;  }</div><div class="line"><a name="l00215"></a><span class="lineno">  215</span>&#160;}</div><div class="line"><a name="l00216"></a><span class="lineno">  216</span>&#160;</div><div class="line"><a name="l00222"></a><span class="lineno"><a class="line" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#af10896911a8035f14939bf2ff5745e29">  222</a></span>&#160;<a class="code" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#aed75690b2aa3bde430ac05521cecb8d1">TEST</a>(MiniCheetah, simulatorFootPosVelMiniCheetah) {</div><div class="line"><a name="l00223"></a><span class="lineno">  223</span>&#160;  <a class="code" href="../../d6/db1/class_floating_base_model.html">FloatingBaseModel&lt;double&gt;</a> cheetahModel =</div><div class="line"><a name="l00224"></a><span class="lineno">  224</span>&#160;      buildMiniCheetah&lt;double&gt;().buildModel();</div><div class="line"><a name="l00225"></a><span class="lineno">  225</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html">DynamicsSimulator&lt;double&gt;</a> sim(cheetahModel);</div><div class="line"><a name="l00226"></a><span class="lineno">  226</span>&#160;</div><div class="line"><a name="l00227"></a><span class="lineno">  227</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat&lt;double&gt;</a> rBody = <a class="code" href="../../da/d9b/namespaceori.html#a537b8d0769c03e4cd8ef2b9a9dc8a760">coordinateRotation</a>(CoordinateAxis::X, .123) *</div><div class="line"><a name="l00228"></a><span class="lineno">  228</span>&#160;                         <a class="code" href="../../da/d9b/namespaceori.html#a537b8d0769c03e4cd8ef2b9a9dc8a760">coordinateRotation</a>(CoordinateAxis::Z, .232) *</div><div class="line"><a name="l00229"></a><span class="lineno">  229</span>&#160;                         <a class="code" href="../../da/d9b/namespaceori.html#a537b8d0769c03e4cd8ef2b9a9dc8a760">coordinateRotation</a>(CoordinateAxis::Y, .111);</div><div class="line"><a name="l00230"></a><span class="lineno">  230</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;double&gt;</a> bodyVel;</div><div class="line"><a name="l00231"></a><span class="lineno">  231</span>&#160;  bodyVel &lt;&lt; 1, 2, 3, 4, 5, 6;</div><div class="line"><a name="l00232"></a><span class="lineno">  232</span>&#160;  <a class="code" href="../../de/d75/struct_f_b_model_state.html">FBModelState&lt;double&gt;</a> x;</div><div class="line"><a name="l00233"></a><span class="lineno">  233</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> q(12);</div><div class="line"><a name="l00234"></a><span class="lineno">  234</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> dq(12);</div><div class="line"><a name="l00235"></a><span class="lineno">  235</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> tau(12);</div><div class="line"><a name="l00236"></a><span class="lineno">  236</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 12; i++) {</div><div class="line"><a name="l00237"></a><span class="lineno">  237</span>&#160;    q[i] = i + 1;</div><div class="line"><a name="l00238"></a><span class="lineno">  238</span>&#160;    dq[i] = (i + 1) * 2;</div><div class="line"><a name="l00239"></a><span class="lineno">  239</span>&#160;    tau[i] = (i + 1) * -30.;</div><div class="line"><a name="l00240"></a><span class="lineno">  240</span>&#160;  }</div><div class="line"><a name="l00241"></a><span class="lineno">  241</span>&#160;</div><div class="line"><a name="l00242"></a><span class="lineno">  242</span>&#160;  <span class="comment">// set state</span></div><div class="line"><a name="l00243"></a><span class="lineno">  243</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#ae42e1f0d68b89af761bcaf273b97bca7">bodyOrientation</a> = <a class="code" href="../../da/d9b/namespaceori.html#a73c3872af6870d856cedd662856fcaea">rotationMatrixToQuaternion</a>(rBody.transpose());</div><div class="line"><a name="l00244"></a><span class="lineno">  244</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abf1fc0221d6b92d103193524a2046ffc">bodyVelocity</a> = bodyVel;</div><div class="line"><a name="l00245"></a><span class="lineno">  245</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a> = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a>(6, 7, 8);</div><div class="line"><a name="l00246"></a><span class="lineno">  246</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#ae5a66b0d3fecb2cdd9ee3c68f72e9182">q</a> = q;</div><div class="line"><a name="l00247"></a><span class="lineno">  247</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#a97f42a6014fc5d9fd3dc22f7158099f4">qd</a> = dq;</div><div class="line"><a name="l00248"></a><span class="lineno">  248</span>&#160;</div><div class="line"><a name="l00249"></a><span class="lineno">  249</span>&#160;  <span class="comment">// generate external forces</span></div><div class="line"><a name="l00250"></a><span class="lineno">  250</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a078e1eaa8a3af13cef27a0a8d0c1302e">vectorAligned&lt;SVec&lt;double&gt;</a>&gt; forces(18);</div><div class="line"><a name="l00251"></a><span class="lineno">  251</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 18; i++) {</div><div class="line"><a name="l00252"></a><span class="lineno">  252</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j = 0; j &lt; 6; j++) {</div><div class="line"><a name="l00253"></a><span class="lineno">  253</span>&#160;      forces[i][j] = i + j + 1;</div><div class="line"><a name="l00254"></a><span class="lineno">  254</span>&#160;    }</div><div class="line"><a name="l00255"></a><span class="lineno">  255</span>&#160;  }</div><div class="line"><a name="l00256"></a><span class="lineno">  256</span>&#160;</div><div class="line"><a name="l00257"></a><span class="lineno">  257</span>&#160;  <span class="comment">// fwd kin is included in this</span></div><div class="line"><a name="l00258"></a><span class="lineno">  258</span>&#160;  sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a1b185d90d2b15840ca57f8e2adcf84fb">setState</a>(x);</div><div class="line"><a name="l00259"></a><span class="lineno">  259</span>&#160;  sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a2310bcadca5b6ed12ec9cee898335625">setAllExternalForces</a>(forces);</div><div class="line"><a name="l00260"></a><span class="lineno">  260</span>&#160;  sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a84b25acaf7edf23f4f437a96a0d8ba05">step</a>(0.0, tau, 5e5, 5e3);</div><div class="line"><a name="l00261"></a><span class="lineno">  261</span>&#160;</div><div class="line"><a name="l00262"></a><span class="lineno">  262</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a> footpRefML(5.4937, 7.1459, 7.8096);</div><div class="line"><a name="l00263"></a><span class="lineno">  263</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a> footvRefML(-2.5284, 2.0944, 16.3732);</div><div class="line"><a name="l00264"></a><span class="lineno">  264</span>&#160;</div><div class="line"><a name="l00265"></a><span class="lineno">  265</span>&#160;  <span class="comment">// I add the body points in a different order, so comparing them is kind of</span></div><div class="line"><a name="l00266"></a><span class="lineno">  266</span>&#160;  <span class="comment">// annoying. this just one foot point.</span></div><div class="line"><a name="l00267"></a><span class="lineno">  267</span>&#160;  EXPECT_TRUE(<a class="code" href="../../de/dda/_math_utilities_8h.html#aa06ec79f1e3170ea41b58ffbf35b9fb6">almostEqual</a>(footpRefML, sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a560cb59e43a731fd2a8bb4f54b4086bd">getModel</a>().<a class="code" href="../../d6/db1/class_floating_base_model.html#a5566ab4692461d4240cec1fdbc2ab948">_pGC</a>.at(15), .0005));</div><div class="line"><a name="l00268"></a><span class="lineno">  268</span>&#160;  EXPECT_TRUE(<a class="code" href="../../de/dda/_math_utilities_8h.html#aa06ec79f1e3170ea41b58ffbf35b9fb6">almostEqual</a>(footvRefML, sim.<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a560cb59e43a731fd2a8bb4f54b4086bd">getModel</a>().<a class="code" href="../../d6/db1/class_floating_base_model.html#a74c3f88954fd3f8884d20a1fb436c8cc">_vGC</a>.at(15), .0005));</div><div class="line"><a name="l00269"></a><span class="lineno">  269</span>&#160;}</div><div class="line"><a name="l00270"></a><span class="lineno">  270</span>&#160;</div><div class="line"><a name="l00274"></a><span class="lineno"><a class="line" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#a9c5b38446558f27a167c30cb9d7af420">  274</a></span>&#160;<a class="code" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#aed75690b2aa3bde430ac05521cecb8d1">TEST</a>(MiniCheetah, hipLocationConvention) {</div><div class="line"><a name="l00275"></a><span class="lineno">  275</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a> hipLocationRef[4];</div><div class="line"><a name="l00276"></a><span class="lineno">  276</span>&#160;  hipLocationRef[0] = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a>(0.19, -0.049, 0.0);</div><div class="line"><a name="l00277"></a><span class="lineno">  277</span>&#160;  hipLocationRef[1] = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a>(0.19, 0.049, 0.0);</div><div class="line"><a name="l00278"></a><span class="lineno">  278</span>&#160;  hipLocationRef[2] = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a>(-0.19, -0.049, 0.0);</div><div class="line"><a name="l00279"></a><span class="lineno">  279</span>&#160;  hipLocationRef[3] = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a>(-0.19, 0.049, 0.0);</div><div class="line"><a name="l00280"></a><span class="lineno">  280</span>&#160;</div><div class="line"><a name="l00281"></a><span class="lineno">  281</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a> hipLocations[4];</div><div class="line"><a name="l00282"></a><span class="lineno">  282</span>&#160;</div><div class="line"><a name="l00283"></a><span class="lineno">  283</span>&#160;  <span class="keyword">auto</span> quadruped = buildMiniCheetah&lt;double&gt;();</div><div class="line"><a name="l00284"></a><span class="lineno">  284</span>&#160;</div><div class="line"><a name="l00285"></a><span class="lineno">  285</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; 4; i++) {</div><div class="line"><a name="l00286"></a><span class="lineno">  286</span>&#160;    hipLocations[i] = quadruped.getHipLocation(i);</div><div class="line"><a name="l00287"></a><span class="lineno">  287</span>&#160;  }</div><div class="line"><a name="l00288"></a><span class="lineno">  288</span>&#160;</div><div class="line"><a name="l00289"></a><span class="lineno">  289</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; 4; i++) {</div><div class="line"><a name="l00290"></a><span class="lineno">  290</span>&#160;    <span class="comment">// std::cout &lt;&lt; &quot;ref: &quot; &lt;&lt; hipLocationRef[i].transpose() &lt;&lt; &quot;\nact: &quot; &lt;&lt;</span></div><div class="line"><a name="l00291"></a><span class="lineno">  291</span>&#160;    <span class="comment">// hipLocations[i].transpose() &lt;&lt; &quot;\n\n&quot;;</span></div><div class="line"><a name="l00292"></a><span class="lineno">  292</span>&#160;    EXPECT_TRUE(<a class="code" href="../../de/dda/_math_utilities_8h.html#aa06ec79f1e3170ea41b58ffbf35b9fb6">almostEqual</a>(hipLocations[i], hipLocationRef[i], .0001));</div><div class="line"><a name="l00293"></a><span class="lineno">  293</span>&#160;  }</div><div class="line"><a name="l00294"></a><span class="lineno">  294</span>&#160;}</div><div class="line"><a name="l00295"></a><span class="lineno">  295</span>&#160;</div><div class="line"><a name="l00296"></a><span class="lineno"><a class="line" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#a8dbf5fb199a7fe2c0c20be271d855ddd">  296</a></span>&#160;<a class="code" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#aed75690b2aa3bde430ac05521cecb8d1">TEST</a>(MiniCheetah, ContactPositionVelocity) {</div><div class="line"><a name="l00297"></a><span class="lineno">  297</span>&#160;  <a class="code" href="../../d6/db1/class_floating_base_model.html">FloatingBaseModel&lt;double&gt;</a> cheetahModel =</div><div class="line"><a name="l00298"></a><span class="lineno">  298</span>&#160;      buildMiniCheetah&lt;double&gt;().buildModel();</div><div class="line"><a name="l00299"></a><span class="lineno">  299</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html">DynamicsSimulator&lt;double&gt;</a> sim(cheetahModel);</div><div class="line"><a name="l00300"></a><span class="lineno">  300</span>&#160;</div><div class="line"><a name="l00301"></a><span class="lineno">  301</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat&lt;double&gt;</a> rBody = <a class="code" href="../../da/d9b/namespaceori.html#a537b8d0769c03e4cd8ef2b9a9dc8a760">coordinateRotation</a>(CoordinateAxis::X, 0.0) *</div><div class="line"><a name="l00302"></a><span class="lineno">  302</span>&#160;                         <a class="code" href="../../da/d9b/namespaceori.html#a537b8d0769c03e4cd8ef2b9a9dc8a760">coordinateRotation</a>(CoordinateAxis::Z, 0.0) *</div><div class="line"><a name="l00303"></a><span class="lineno">  303</span>&#160;                         <a class="code" href="../../da/d9b/namespaceori.html#a537b8d0769c03e4cd8ef2b9a9dc8a760">coordinateRotation</a>(CoordinateAxis::Y, 0.0);</div><div class="line"><a name="l00304"></a><span class="lineno">  304</span>&#160;</div><div class="line"><a name="l00305"></a><span class="lineno">  305</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;double&gt;</a> bodyVel;</div><div class="line"><a name="l00306"></a><span class="lineno">  306</span>&#160;  bodyVel &lt;&lt; 0., 0., 0., 1.0, 0., 0.;</div><div class="line"><a name="l00307"></a><span class="lineno">  307</span>&#160;  <a class="code" href="../../de/d75/struct_f_b_model_state.html">FBModelState&lt;double&gt;</a> x;</div><div class="line"><a name="l00308"></a><span class="lineno">  308</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> q(12);</div><div class="line"><a name="l00309"></a><span class="lineno">  309</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> dq(12);</div><div class="line"><a name="l00310"></a><span class="lineno">  310</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> tau(12);</div><div class="line"><a name="l00311"></a><span class="lineno">  311</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 12; i++) {</div><div class="line"><a name="l00312"></a><span class="lineno">  312</span>&#160;    q[i] = i + 1;</div><div class="line"><a name="l00313"></a><span class="lineno">  313</span>&#160;    dq[i] = (i + 1) * 2;</div><div class="line"><a name="l00314"></a><span class="lineno">  314</span>&#160;    tau[i] = (i + 1) * -30.;</div><div class="line"><a name="l00315"></a><span class="lineno">  315</span>&#160;  }</div><div class="line"><a name="l00316"></a><span class="lineno">  316</span>&#160;</div><div class="line"><a name="l00317"></a><span class="lineno">  317</span>&#160;  <span class="comment">// set state</span></div><div class="line"><a name="l00318"></a><span class="lineno">  318</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#ae42e1f0d68b89af761bcaf273b97bca7">bodyOrientation</a> = <a class="code" href="../../da/d9b/namespaceori.html#a73c3872af6870d856cedd662856fcaea">rotationMatrixToQuaternion</a>(rBody.transpose());</div><div class="line"><a name="l00319"></a><span class="lineno">  319</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abf1fc0221d6b92d103193524a2046ffc">bodyVelocity</a> = bodyVel;</div><div class="line"><a name="l00320"></a><span class="lineno">  320</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a> = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a>(6, 7, 8);</div><div class="line"><a name="l00321"></a><span class="lineno">  321</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#ae5a66b0d3fecb2cdd9ee3c68f72e9182">q</a> = q;</div><div class="line"><a name="l00322"></a><span class="lineno">  322</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#a97f42a6014fc5d9fd3dc22f7158099f4">qd</a> = dq;</div><div class="line"><a name="l00323"></a><span class="lineno">  323</span>&#160;</div><div class="line"><a name="l00324"></a><span class="lineno">  324</span>&#160;  <span class="comment">// generate external forces</span></div><div class="line"><a name="l00325"></a><span class="lineno">  325</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a078e1eaa8a3af13cef27a0a8d0c1302e">vectorAligned&lt;SVec&lt;double&gt;</a>&gt; forces(18);</div><div class="line"><a name="l00326"></a><span class="lineno">  326</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 18; i++) {</div><div class="line"><a name="l00327"></a><span class="lineno">  327</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j = 0; j &lt; 6; j++) {</div><div class="line"><a name="l00328"></a><span class="lineno">  328</span>&#160;      forces[i][j] = i + j + 1;</div><div class="line"><a name="l00329"></a><span class="lineno">  329</span>&#160;    }</div><div class="line"><a name="l00330"></a><span class="lineno">  330</span>&#160;  }</div><div class="line"><a name="l00331"></a><span class="lineno">  331</span>&#160;</div><div class="line"><a name="l00332"></a><span class="lineno">  332</span>&#160;  <span class="comment">// fwd kin is included in this</span></div><div class="line"><a name="l00333"></a><span class="lineno">  333</span>&#160;  cheetahModel.<a class="code" href="../../d6/db1/class_floating_base_model.html#a575819811abf1d4b8e527b22aead4013">setState</a>(x);</div><div class="line"><a name="l00334"></a><span class="lineno">  334</span>&#160;  cheetahModel.<a class="code" href="../../d6/db1/class_floating_base_model.html#a4c4c27d3bc26c3ad1a3306d16b57f085">forwardKinematics</a>();  <span class="comment">// compute forward kinematics</span></div><div class="line"><a name="l00335"></a><span class="lineno">  335</span>&#160;</div><div class="line"><a name="l00336"></a><span class="lineno">  336</span>&#160;  <span class="keywordtype">double</span> length(0.19);</div><div class="line"><a name="l00337"></a><span class="lineno">  337</span>&#160;  <span class="keywordtype">double</span> width(0.049);</div><div class="line"><a name="l00338"></a><span class="lineno">  338</span>&#160;</div><div class="line"><a name="l00339"></a><span class="lineno">  339</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a> FR_abd_ref(x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a>[0] + length, x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a>[1] - width,</div><div class="line"><a name="l00340"></a><span class="lineno">  340</span>&#160;                          x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a>[2]);</div><div class="line"><a name="l00341"></a><span class="lineno">  341</span>&#160;</div><div class="line"><a name="l00342"></a><span class="lineno">  342</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a> FL_abd_ref(x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a>[0] + length, x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a>[1] + width,</div><div class="line"><a name="l00343"></a><span class="lineno">  343</span>&#160;                          x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a>[2]);</div><div class="line"><a name="l00344"></a><span class="lineno">  344</span>&#160;</div><div class="line"><a name="l00345"></a><span class="lineno">  345</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a> HR_abd_ref(x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a>[0] - length, x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a>[1] - width,</div><div class="line"><a name="l00346"></a><span class="lineno">  346</span>&#160;                          x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a>[2]);</div><div class="line"><a name="l00347"></a><span class="lineno">  347</span>&#160;</div><div class="line"><a name="l00348"></a><span class="lineno">  348</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a> HL_abd_ref(x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a>[0] - length, x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a>[1] + width,</div><div class="line"><a name="l00349"></a><span class="lineno">  349</span>&#160;                          x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a>[2]);</div><div class="line"><a name="l00350"></a><span class="lineno">  350</span>&#160;</div><div class="line"><a name="l00351"></a><span class="lineno">  351</span>&#160;  <span class="comment">// for(size_t i(0); i&lt;8; ++i){</span></div><div class="line"><a name="l00352"></a><span class="lineno">  352</span>&#160;  <span class="comment">// printf(&quot;%lu th\n&quot;, i);</span></div><div class="line"><a name="l00353"></a><span class="lineno">  353</span>&#160;  <span class="comment">// pretty_print(cheetahModel._pGC.at(i), std::cout, &quot;cp pos&quot;);</span></div><div class="line"><a name="l00354"></a><span class="lineno">  354</span>&#160;  <span class="comment">//}</span></div><div class="line"><a name="l00355"></a><span class="lineno">  355</span>&#160;</div><div class="line"><a name="l00356"></a><span class="lineno">  356</span>&#160;  <span class="comment">// I add the body points in a different order, so comparing them is kind of</span></div><div class="line"><a name="l00357"></a><span class="lineno">  357</span>&#160;  <span class="comment">// annoying. this just one foot point.</span></div><div class="line"><a name="l00358"></a><span class="lineno">  358</span>&#160;  EXPECT_TRUE(</div><div class="line"><a name="l00359"></a><span class="lineno">  359</span>&#160;      <a class="code" href="../../de/dda/_math_utilities_8h.html#aa06ec79f1e3170ea41b58ffbf35b9fb6">almostEqual</a>(FR_abd_ref, cheetahModel.<a class="code" href="../../d6/db1/class_floating_base_model.html#a5566ab4692461d4240cec1fdbc2ab948">_pGC</a>.at(<a class="code" href="../../dc/d53/namespacelink_i_d.html#a9fe3d0127d71419a83f3b592e445916c">linkID::FR_abd</a>), .0005));</div><div class="line"><a name="l00360"></a><span class="lineno">  360</span>&#160;  EXPECT_TRUE(</div><div class="line"><a name="l00361"></a><span class="lineno">  361</span>&#160;      <a class="code" href="../../de/dda/_math_utilities_8h.html#aa06ec79f1e3170ea41b58ffbf35b9fb6">almostEqual</a>(FL_abd_ref, cheetahModel.<a class="code" href="../../d6/db1/class_floating_base_model.html#a5566ab4692461d4240cec1fdbc2ab948">_pGC</a>.at(<a class="code" href="../../dc/d53/namespacelink_i_d.html#af1caf5f2fcd86b328a8b143efd30198d">linkID::FL_abd</a>), .0005));</div><div class="line"><a name="l00362"></a><span class="lineno">  362</span>&#160;  EXPECT_TRUE(</div><div class="line"><a name="l00363"></a><span class="lineno">  363</span>&#160;      <a class="code" href="../../de/dda/_math_utilities_8h.html#aa06ec79f1e3170ea41b58ffbf35b9fb6">almostEqual</a>(HR_abd_ref, cheetahModel.<a class="code" href="../../d6/db1/class_floating_base_model.html#a5566ab4692461d4240cec1fdbc2ab948">_pGC</a>.at(<a class="code" href="../../dc/d53/namespacelink_i_d.html#ab7737d4eb5e5c0f261ebb82a6bd57ab1">linkID::HR_abd</a>), .0005));</div><div class="line"><a name="l00364"></a><span class="lineno">  364</span>&#160;  EXPECT_TRUE(</div><div class="line"><a name="l00365"></a><span class="lineno">  365</span>&#160;      <a class="code" href="../../de/dda/_math_utilities_8h.html#aa06ec79f1e3170ea41b58ffbf35b9fb6">almostEqual</a>(HL_abd_ref, cheetahModel.<a class="code" href="../../d6/db1/class_floating_base_model.html#a5566ab4692461d4240cec1fdbc2ab948">_pGC</a>.at(<a class="code" href="../../dc/d53/namespacelink_i_d.html#adc10b97ff6188ddcb914c6ff6cf82a3c">linkID::HL_abd</a>), .0005));</div><div class="line"><a name="l00366"></a><span class="lineno">  366</span>&#160;}</div><div class="line"><a name="l00367"></a><span class="lineno">  367</span>&#160;</div><div class="line"><a name="l00368"></a><span class="lineno"><a class="line" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#ae15c69cb4f98e6a32d74511320aac05c">  368</a></span>&#160;<a class="code" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#aed75690b2aa3bde430ac05521cecb8d1">TEST</a>(MiniCheetah, InertiaProperty) {</div><div class="line"><a name="l00369"></a><span class="lineno">  369</span>&#160;  <a class="code" href="../../d6/db1/class_floating_base_model.html">FloatingBaseModel&lt;double&gt;</a> cheetahModel =</div><div class="line"><a name="l00370"></a><span class="lineno">  370</span>&#160;      buildMiniCheetah&lt;double&gt;().buildModel();</div><div class="line"><a name="l00371"></a><span class="lineno">  371</span>&#160;</div><div class="line"><a name="l00372"></a><span class="lineno">  372</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;double&gt;</a> bodyVel;</div><div class="line"><a name="l00373"></a><span class="lineno">  373</span>&#160;  <a class="code" href="../../de/d75/struct_f_b_model_state.html">FBModelState&lt;double&gt;</a> x;</div><div class="line"><a name="l00374"></a><span class="lineno">  374</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> q(12);</div><div class="line"><a name="l00375"></a><span class="lineno">  375</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> dq(12);</div><div class="line"><a name="l00376"></a><span class="lineno">  376</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;double&gt;</a> tau(12);</div><div class="line"><a name="l00377"></a><span class="lineno">  377</span>&#160;</div><div class="line"><a name="l00378"></a><span class="lineno">  378</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 12; i++) {</div><div class="line"><a name="l00379"></a><span class="lineno">  379</span>&#160;    q[i] = 0.;</div><div class="line"><a name="l00380"></a><span class="lineno">  380</span>&#160;    dq[i] = 0.;</div><div class="line"><a name="l00381"></a><span class="lineno">  381</span>&#160;    tau[i] = 0.;</div><div class="line"><a name="l00382"></a><span class="lineno">  382</span>&#160;  }</div><div class="line"><a name="l00383"></a><span class="lineno">  383</span>&#160;  q[1] = -M_PI / 2.;</div><div class="line"><a name="l00384"></a><span class="lineno">  384</span>&#160;  q[4] = -M_PI / 2.;</div><div class="line"><a name="l00385"></a><span class="lineno">  385</span>&#160;  q[7] = M_PI / 2.;</div><div class="line"><a name="l00386"></a><span class="lineno">  386</span>&#160;  q[10] = M_PI / 2.;</div><div class="line"><a name="l00387"></a><span class="lineno">  387</span>&#160;</div><div class="line"><a name="l00388"></a><span class="lineno">  388</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;double&gt;</a> rpy_ori;</div><div class="line"><a name="l00389"></a><span class="lineno">  389</span>&#160;  rpy_ori.setZero();</div><div class="line"><a name="l00390"></a><span class="lineno">  390</span>&#160;  <span class="comment">// set state</span></div><div class="line"><a name="l00391"></a><span class="lineno">  391</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#ae42e1f0d68b89af761bcaf273b97bca7">bodyOrientation</a> = <a class="code" href="../../da/d9b/namespaceori.html#ade40aba34aa5e6b4f82753eb3c63d16a">ori::rpyToQuat</a>(rpy_ori);</div><div class="line"><a name="l00392"></a><span class="lineno">  392</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abf1fc0221d6b92d103193524a2046ffc">bodyVelocity</a>.setZero();</div><div class="line"><a name="l00393"></a><span class="lineno">  393</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">bodyPosition</a>.setZero();</div><div class="line"><a name="l00394"></a><span class="lineno">  394</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#ae5a66b0d3fecb2cdd9ee3c68f72e9182">q</a> = q;</div><div class="line"><a name="l00395"></a><span class="lineno">  395</span>&#160;  x.<a class="code" href="../../de/d75/struct_f_b_model_state.html#a97f42a6014fc5d9fd3dc22f7158099f4">qd</a> = dq;</div><div class="line"><a name="l00396"></a><span class="lineno">  396</span>&#160;</div><div class="line"><a name="l00397"></a><span class="lineno">  397</span>&#160;  cheetahModel.<a class="code" href="../../d6/db1/class_floating_base_model.html#a575819811abf1d4b8e527b22aead4013">setState</a>(x);</div><div class="line"><a name="l00398"></a><span class="lineno">  398</span>&#160;  cheetahModel.<a class="code" href="../../d6/db1/class_floating_base_model.html#a73d58fada538f7d74ad94c48e5024b78">massMatrix</a>();</div><div class="line"><a name="l00399"></a><span class="lineno">  399</span>&#160;</div><div class="line"><a name="l00400"></a><span class="lineno">  400</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a807200f3771dbd2cb2e4be52c5f2518c">DMat&lt;double&gt;</a> H = cheetahModel.<a class="code" href="../../d6/db1/class_floating_base_model.html#a98a73af4cee379189bc5e85ad502ff2d">getMassMatrix</a>();</div><div class="line"><a name="l00401"></a><span class="lineno">  401</span>&#160;</div><div class="line"><a name="l00402"></a><span class="lineno">  402</span>&#160;  <span class="comment">// pretty_print(H, std::cout, &quot;H&quot;);</span></div><div class="line"><a name="l00403"></a><span class="lineno">  403</span>&#160;  <span class="comment">// exit(0);</span></div><div class="line"><a name="l00404"></a><span class="lineno">  404</span>&#160;}</div><div class="ttc" id="struct_f_b_model_state_html_abbf400085816b6baa4e4a0597ac5b118"><div class="ttname"><a href="../../de/d75/struct_f_b_model_state.html#abbf400085816b6baa4e4a0597ac5b118">FBModelState::bodyPosition</a></div><div class="ttdeci">Vec3&lt; T &gt; bodyPosition</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00041">FloatingBaseModel.h:41</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a647e76b4459b43335cf528795bab72d5"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a647e76b4459b43335cf528795bab72d5">FloatingBaseModel::_Xrot</a></div><div class="ttdeci">vector&lt; Mat6&lt; T &gt;, Eigen::aligned_allocator&lt; Mat6&lt; T &gt; &gt; &gt; _Xrot</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00246">FloatingBaseModel.h:246</a></div></div>
<div class="ttc" id="namespacecheetah_html"><div class="ttname"><a href="../../d7/dfa/namespacecheetah.html">cheetah</a></div><div class="ttdef"><b>Definition:</b> <a href="../../d3/dc7/_quadruped_8h_source.html#l00021">Quadruped.h:21</a></div></div>
<div class="ttc" id="utilities_8h_html_a16ea9aab4737c2622bc370d6a8613d63"><div class="ttname"><a href="../../de/df0/utilities_8h.html#a16ea9aab4737c2622bc370d6a8613d63">fpEqual</a></div><div class="ttdeci">bool fpEqual(T a, T b, T tol)</div><div class="ttdef"><b>Definition:</b> <a href="../../de/df0/utilities_8h_source.html#l00015">utilities.h:15</a></div></div>
<div class="ttc" id="namespacelink_i_d_html_adc10b97ff6188ddcb914c6ff6cf82a3c"><div class="ttname"><a href="../../dc/d53/namespacelink_i_d.html#adc10b97ff6188ddcb914c6ff6cf82a3c">linkID::HL_abd</a></div><div class="ttdeci">constexpr size_t HL_abd</div><div class="ttdef"><b>Definition:</b> <a href="../../d3/dc7/_quadruped_8h_source.html#l00039">Quadruped.h:39</a></div></div>
<div class="ttc" id="namespacelink_i_d_html_af1caf5f2fcd86b328a8b143efd30198d"><div class="ttname"><a href="../../dc/d53/namespacelink_i_d.html#af1caf5f2fcd86b328a8b143efd30198d">linkID::FL_abd</a></div><div class="ttdeci">constexpr size_t FL_abd</div><div class="ttdef"><b>Definition:</b> <a href="../../d3/dc7/_quadruped_8h_source.html#l00037">Quadruped.h:37</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_a3c057386e8d437e8c5dbcae02acf935c"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#a3c057386e8d437e8c5dbcae02acf935c">DynamicsSimulator::forwardKinematics</a></div><div class="ttdeci">void forwardKinematics()</div><div class="ttdoc">Do forward kinematics for feet. </div><div class="ttdef"><b>Definition:</b> <a href="../../d0/d52/_dynamics_simulator_8h_source.html#l00053">DynamicsSimulator.h:53</a></div></div>
<div class="ttc" id="struct_f_b_model_state_html_abf1fc0221d6b92d103193524a2046ffc"><div class="ttname"><a href="../../de/d75/struct_f_b_model_state.html#abf1fc0221d6b92d103193524a2046ffc">FBModelState::bodyVelocity</a></div><div class="ttdeci">SVec&lt; T &gt; bodyVelocity</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00042">FloatingBaseModel.h:42</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html">DynamicsSimulator&lt; double &gt;</a></div></div>
<div class="ttc" id="namespaceori_html_a537b8d0769c03e4cd8ef2b9a9dc8a760"><div class="ttname"><a href="../../da/d9b/namespaceori.html#a537b8d0769c03e4cd8ef2b9a9dc8a760">ori::coordinateRotation</a></div><div class="ttdeci">Mat3&lt; T &gt; coordinateRotation(CoordinateAxis axis, T theta)</div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00060">orientation_tools.h:60</a></div></div>
<div class="ttc" id="utilities_8h_html"><div class="ttname"><a href="../../de/df0/utilities_8h.html">utilities.h</a></div></div>
<div class="ttc" id="cpp_types_8h_html_ae8ab6bd4079a5317297be5eeb331b1ca"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 6, 6 &gt; Mat6</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00070">cppTypes.h:70</a></div></div>
<div class="ttc" id="struct_f_b_model_state_derivative_html_a825cfc02adde2c48b0d508a3bf77e896"><div class="ttname"><a href="../../de/dce/struct_f_b_model_state_derivative.html#a825cfc02adde2c48b0d508a3bf77e896">FBModelStateDerivative::dBodyPosition</a></div><div class="ttdeci">EIGEN_MAKE_ALIGNED_OPERATOR_NEW Vec3&lt; T &gt; dBodyPosition</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00059">FloatingBaseModel.h:59</a></div></div>
<div class="ttc" id="_quadruped_8h_html"><div class="ttname"><a href="../../d3/dc7/_quadruped_8h.html">Quadruped.h</a></div><div class="ttdoc">Data structure containing parameters for quadruped robot. </div></div>
<div class="ttc" id="cpp_types_8h_html_a807200f3771dbd2cb2e4be52c5f2518c"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a807200f3771dbd2cb2e4be52c5f2518c">DMat</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, Eigen::Dynamic, Eigen::Dynamic &gt; DMat</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00106">cppTypes.h:106</a></div></div>
<div class="ttc" id="test__mini__cheetah__model_8cpp_html_aed75690b2aa3bde430ac05521cecb8d1"><div class="ttname"><a href="../../d6/d6e/test__mini__cheetah__model_8cpp.html#aed75690b2aa3bde430ac05521cecb8d1">TEST</a></div><div class="ttdeci">TEST(MiniCheetah, miniCheetahModel1)</div><div class="ttdef"><b>Definition:</b> <a href="../../d6/d6e/test__mini__cheetah__model_8cpp_source.html#l00025">test_mini_cheetah_model.cpp:25</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a477c5193fa0c0f6e36314eca214a7248"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a477c5193fa0c0f6e36314eca214a7248">FloatingBaseModel::getParentVector</a></div><div class="ttdeci">const std::vector&lt; int &gt; &amp; getParentVector()</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00154">FloatingBaseModel.h:154</a></div></div>
<div class="ttc" id="struct_f_b_model_state_html_ae5a66b0d3fecb2cdd9ee3c68f72e9182"><div class="ttname"><a href="../../de/d75/struct_f_b_model_state.html#ae5a66b0d3fecb2cdd9ee3c68f72e9182">FBModelState::q</a></div><div class="ttdeci">DVec&lt; T &gt; q</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00043">FloatingBaseModel.h:43</a></div></div>
<div class="ttc" id="class_floating_base_model_html_afb96756832c10edf60bcb16a0a1589bd"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#afb96756832c10edf60bcb16a0a1589bd">FloatingBaseModel::_Xtree</a></div><div class="ttdeci">vector&lt; Mat6&lt; T &gt;, Eigen::aligned_allocator&lt; Mat6&lt; T &gt; &gt; &gt; _Xtree</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00246">FloatingBaseModel.h:246</a></div></div>
<div class="ttc" id="struct_f_b_model_state_html_a97f42a6014fc5d9fd3dc22f7158099f4"><div class="ttname"><a href="../../de/d75/struct_f_b_model_state.html#a97f42a6014fc5d9fd3dc22f7158099f4">FBModelState::qd</a></div><div class="ttdeci">DVec&lt; T &gt; qd</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00044">FloatingBaseModel.h:44</a></div></div>
<div class="ttc" id="_mini_cheetah_8h_html"><div class="ttname"><a href="../../d8/db6/_mini_cheetah_8h.html">MiniCheetah.h</a></div><div class="ttdoc">Utility function to build a Mini Cheetah Quadruped object. </div></div>
<div class="ttc" id="cpp_types_8h_html_a6f07728c866b15ff0676a9b2ebfd2ba6"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 3, 1 &gt; Vec3</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00026">cppTypes.h:26</a></div></div>
<div class="ttc" id="struct_f_b_model_state_derivative_html_a8b7b6d70f1b1268f02e1b291dad26c5d"><div class="ttname"><a href="../../de/dce/struct_f_b_model_state_derivative.html#a8b7b6d70f1b1268f02e1b291dad26c5d">FBModelStateDerivative::dBodyVelocity</a></div><div class="ttdeci">SVec&lt; T &gt; dBodyVelocity</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00060">FloatingBaseModel.h:60</a></div></div>
<div class="ttc" id="_utilities__print_8h_html"><div class="ttname"><a href="../../d3/d68/_utilities__print_8h.html">Utilities_print.h</a></div></div>
<div class="ttc" id="namespacelink_i_d_html_a9fe3d0127d71419a83f3b592e445916c"><div class="ttname"><a href="../../dc/d53/namespacelink_i_d.html#a9fe3d0127d71419a83f3b592e445916c">linkID::FR_abd</a></div><div class="ttdeci">constexpr size_t FR_abd</div><div class="ttdef"><b>Definition:</b> <a href="../../d3/dc7/_quadruped_8h_source.html#l00036">Quadruped.h:36</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a445bbce9a3f0a56164b06d39760d2383"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a445bbce9a3f0a56164b06d39760d2383">FloatingBaseModel::getBodyInertiaVector</a></div><div class="ttdeci">const std::vector&lt; SpatialInertia&lt; T &gt;, Eigen::aligned_allocator&lt; SpatialInertia&lt; T &gt; &gt; &gt; &amp; getBodyInertiaVector()</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00158">FloatingBaseModel.h:158</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a74c3f88954fd3f8884d20a1fb436c8cc"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a74c3f88954fd3f8884d20a1fb436c8cc">FloatingBaseModel::_vGC</a></div><div class="ttdeci">vector&lt; Vec3&lt; T &gt; &gt; _vGC</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00257">FloatingBaseModel.h:257</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_a1b185d90d2b15840ca57f8e2adcf84fb"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#a1b185d90d2b15840ca57f8e2adcf84fb">DynamicsSimulator::setState</a></div><div class="ttdeci">void setState(const FBModelState&lt; T &gt; &amp;state)</div><div class="ttdef"><b>Definition:</b> <a href="../../d0/d52/_dynamics_simulator_8h_source.html#l00061">DynamicsSimulator.h:61</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a3d70f8eb9cf1855ca989f887a5543acc"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 6, 1 &gt; SVec</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00062">cppTypes.h:62</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a98a73af4cee379189bc5e85ad502ff2d"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a98a73af4cee379189bc5e85ad502ff2d">FloatingBaseModel::getMassMatrix</a></div><div class="ttdeci">const DMat&lt; T &gt; &amp; getMassMatrix() const </div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00261">FloatingBaseModel.h:261</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a4c77595286da687122899d5e61139305"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a4c77595286da687122899d5e61139305">FloatingBaseModel::getRotorInertiaVector</a></div><div class="ttdeci">const std::vector&lt; SpatialInertia&lt; T &gt;, Eigen::aligned_allocator&lt; SpatialInertia&lt; T &gt; &gt; &gt; &amp; getRotorInertiaVector()</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00164">FloatingBaseModel.h:164</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a76cd0fcd431c77e002d8e111b2880cbe"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a76cd0fcd431c77e002d8e111b2880cbe">FloatingBaseModel::totalRotorMass</a></div><div class="ttdeci">T totalRotorMass()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00465">FloatingBaseModel.cpp:465</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_a2310bcadca5b6ed12ec9cee898335625"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#a2310bcadca5b6ed12ec9cee898335625">DynamicsSimulator::setAllExternalForces</a></div><div class="ttdeci">void setAllExternalForces(const vectorAligned&lt; SVec&lt; T &gt;&gt; &amp;forces)</div><div class="ttdef"><b>Definition:</b> <a href="../../d0/d52/_dynamics_simulator_8h_source.html#l00087">DynamicsSimulator.h:87</a></div></div>
<div class="ttc" id="struct_f_b_model_state_html"><div class="ttname"><a href="../../de/d75/struct_f_b_model_state.html">FBModelState&lt; double &gt;</a></div></div>
<div class="ttc" id="namespaceori_html_a73c3872af6870d856cedd662856fcaea"><div class="ttname"><a href="../../da/d9b/namespaceori.html#a73c3872af6870d856cedd662856fcaea">ori::rotationMatrixToQuaternion</a></div><div class="ttdeci">Quat&lt; typename T::Scalar &gt; rotationMatrixToQuaternion(const Eigen::MatrixBase&lt; T &gt; &amp;r1)</div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00119">orientation_tools.h:119</a></div></div>
<div class="ttc" id="_math_utilities_8h_html_aa06ec79f1e3170ea41b58ffbf35b9fb6"><div class="ttname"><a href="../../de/dda/_math_utilities_8h.html#aa06ec79f1e3170ea41b58ffbf35b9fb6">almostEqual</a></div><div class="ttdeci">bool almostEqual(const Eigen::MatrixBase&lt; T &gt; &amp;a, const Eigen::MatrixBase&lt; T &gt; &amp;b, T2 tol)</div><div class="ttdef"><b>Definition:</b> <a href="../../de/dda/_math_utilities_8h_source.html#l00023">MathUtilities.h:23</a></div></div>
<div class="ttc" id="namespacelink_i_d_html_ab7737d4eb5e5c0f261ebb82a6bd57ab1"><div class="ttname"><a href="../../dc/d53/namespacelink_i_d.html#ab7737d4eb5e5c0f261ebb82a6bd57ab1">linkID::HR_abd</a></div><div class="ttdeci">constexpr size_t HR_abd</div><div class="ttdef"><b>Definition:</b> <a href="../../d3/dc7/_quadruped_8h_source.html#l00038">Quadruped.h:38</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_a84b25acaf7edf23f4f437a96a0d8ba05"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#a84b25acaf7edf23f4f437a96a0d8ba05">DynamicsSimulator::step</a></div><div class="ttdeci">void step(T dt, const DVec&lt; T &gt; &amp;tau, T kp, T kd)</div><div class="ttdoc">Initialize simulator with given model. </div><div class="ttdef"><b>Definition:</b> <a href="../../de/d4c/_dynamics_simulator_8cpp_source.html#l00041">DynamicsSimulator.cpp:41</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_ab7c1b6d1eb8d4893458b6910b1f9d331"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#ab7c1b6d1eb8d4893458b6910b1f9d331">DynamicsSimulator::runABA</a></div><div class="ttdeci">void runABA(const DVec&lt; T &gt; &amp;tau)</div><div class="ttdoc">Simulate forward one step. </div><div class="ttdef"><b>Definition:</b> <a href="../../d0/d52/_dynamics_simulator_8h_source.html#l00050">DynamicsSimulator.h:50</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a4c4c27d3bc26c3ad1a3306d16b57f085"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a4c4c27d3bc26c3ad1a3306d16b57f085">FloatingBaseModel::forwardKinematics</a></div><div class="ttdeci">void forwardKinematics()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00479">FloatingBaseModel.cpp:479</a></div></div>
<div class="ttc" id="utilities_8h_html_a42a85233a23e8e7af211cb76f5fce9ee"><div class="ttname"><a href="../../de/df0/utilities_8h.html#a42a85233a23e8e7af211cb76f5fce9ee">vectorEqual</a></div><div class="ttdeci">bool vectorEqual(const std::vector&lt; T &gt; &amp;a, const std::vector&lt; T &gt; &amp;b)</div><div class="ttdef"><b>Definition:</b> <a href="../../de/df0/utilities_8h_source.html#l00023">utilities.h:23</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a575819811abf1d4b8e527b22aead4013"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a575819811abf1d4b8e527b22aead4013">FloatingBaseModel::setState</a></div><div class="ttdeci">void setState(const FBModelState&lt; T &gt; &amp;state)</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00191">FloatingBaseModel.h:191</a></div></div>
<div class="ttc" id="namespaceori_html_ade40aba34aa5e6b4f82753eb3c63d16a"><div class="ttname"><a href="../../da/d9b/namespaceori.html#ade40aba34aa5e6b4f82753eb3c63d16a">ori::rpyToQuat</a></div><div class="ttdeci">Quat&lt; typename T::Scalar &gt; rpyToQuat(const Eigen::MatrixBase&lt; T &gt; &amp;rpy)</div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00201">orientation_tools.h:201</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a078e1eaa8a3af13cef27a0a8d0c1302e"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a078e1eaa8a3af13cef27a0a8d0c1302e">vectorAligned</a></div><div class="ttdeci">typename std::vector&lt; T, Eigen::aligned_allocator&lt; T &gt;&gt; vectorAligned</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00118">cppTypes.h:118</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a5566ab4692461d4240cec1fdbc2ab948"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a5566ab4692461d4240cec1fdbc2ab948">FloatingBaseModel::_pGC</a></div><div class="ttdeci">vector&lt; Vec3&lt; T &gt; &gt; _pGC</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00256">FloatingBaseModel.h:256</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_a1dc9f92d57994e053f91b9cb619adde4"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#a1dc9f92d57994e053f91b9cb619adde4">DynamicsSimulator::getDState</a></div><div class="ttdeci">const FBModelStateDerivative&lt; T &gt; &amp; getDState() const </div><div class="ttdef"><b>Definition:</b> <a href="../../d0/d52/_dynamics_simulator_8h_source.html#l00080">DynamicsSimulator.h:80</a></div></div>
<div class="ttc" id="namespacespatial_html"><div class="ttname"><a href="../../de/d1a/namespacespatial.html">spatial</a></div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00019">spatial.h:19</a></div></div>
<div class="ttc" id="class_floating_base_model_html"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html">FloatingBaseModel&lt; double &gt;</a></div></div>
<div class="ttc" id="struct_f_b_model_state_derivative_html_af32a2041b72e6b30ed1c8a027d340fd2"><div class="ttname"><a href="../../de/dce/struct_f_b_model_state_derivative.html#af32a2041b72e6b30ed1c8a027d340fd2">FBModelStateDerivative::qdd</a></div><div class="ttdeci">DVec&lt; T &gt; qdd</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00061">FloatingBaseModel.h:61</a></div></div>
<div class="ttc" id="cpp_types_8h_html_ae2e5a0a178a2c528d8aa939f85db7a3e"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, Eigen::Dynamic, 1 &gt; DVec</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00102">cppTypes.h:102</a></div></div>
<div class="ttc" id="class_floating_base_model_html_ac3e58feb2dceedb9739bc76bcaccbc98"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#ac3e58feb2dceedb9739bc76bcaccbc98">FloatingBaseModel::totalNonRotorMass</a></div><div class="ttdeci">T totalNonRotorMass()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00452">FloatingBaseModel.cpp:452</a></div></div>
<div class="ttc" id="_dynamics_simulator_8h_html"><div class="ttname"><a href="../../d0/d52/_dynamics_simulator_8h.html">DynamicsSimulator.h</a></div><div class="ttdoc">Rigid Body Dynamics Simulator with Collisions. </div></div>
<div class="ttc" id="class_dynamics_simulator_html_a560cb59e43a731fd2a8bb4f54b4086bd"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#a560cb59e43a731fd2a8bb4f54b4086bd">DynamicsSimulator::getModel</a></div><div class="ttdeci">const FloatingBaseModel&lt; T &gt; &amp; getModel()</div><div class="ttdef"><b>Definition:</b> <a href="../../d0/d52/_dynamics_simulator_8h_source.html#l00118">DynamicsSimulator.h:118</a></div></div>
<div class="ttc" id="class_floating_base_model_html_a73d58fada538f7d74ad94c48e5024b78"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html#a73d58fada538f7d74ad94c48e5024b78">FloatingBaseModel::massMatrix</a></div><div class="ttdeci">DMat&lt; T &gt; massMatrix()</div><div class="ttdef"><b>Definition:</b> <a href="../../da/de7/_floating_base_model_8cpp_source.html#l00778">FloatingBaseModel.cpp:778</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a0b83d93a3c74d282558436e26c056896"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 3, 3 &gt; RotMat</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00018">cppTypes.h:18</a></div></div>
<div class="ttc" id="_floating_base_model_8h_html"><div class="ttname"><a href="../../d5/d9a/_floating_base_model_8h.html">FloatingBaseModel.h</a></div><div class="ttdoc">Implementation of Rigid Body Floating Base model data structure. </div></div>
<div class="ttc" id="struct_f_b_model_state_html_ae42e1f0d68b89af761bcaf273b97bca7"><div class="ttname"><a href="../../de/d75/struct_f_b_model_state.html#ae42e1f0d68b89af761bcaf273b97bca7">FBModelState::bodyOrientation</a></div><div class="ttdeci">EIGEN_MAKE_ALIGNED_OPERATOR_NEW Quat&lt; T &gt; bodyOrientation</div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00040">FloatingBaseModel.h:40</a></div></div>
</div><!-- fragment --></div><!-- contents -->
</div><!-- doc-content -->
<!-- start footer part -->
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
  <ul>
    <li class="navelem"><a class="el" href="../../dir_43e0a1f539e00dcfa1a6bc4d4fee4fc2.html">home</a></li><li class="navelem"><a class="el" href="../../dir_61bfb0b3cdda81b1b0355ba34eb3b1eb.html">dhkim</a></li><li class="navelem"><a class="el" href="../../dir_adee5b101e079090017bb4e91c628ead.html">Public_Repository</a></li><li class="navelem"><a class="el" href="../../dir_7fc07a7b97001e7b1d18e11da761e2de.html">Cheetah-Software</a></li><li class="navelem"><a class="el" href="../../dir_860379c26524a894b3cf97b5b1ba5bdb.html">common</a></li><li class="navelem"><a class="el" href="../../dir_bcd721272d4e8dced7bf80a08d191b42.html">test</a></li><li class="navelem"><a class="el" href="../../d6/d6e/test__mini__cheetah__model_8cpp.html">test_mini_cheetah_model.cpp</a></li>
    <li class="footer">Generated by
    <a href="http://www.doxygen.org/index.html">
    <img class="footer" src="../../doxygen.png" alt="doxygen"/></a> 1.8.11 </li>
  </ul>
</div>
</body>
</html>
